// app
#include "robot_def.h"
#include "robot_cmd.h"
// module
#include "remote_control.h"
#include "ins_task.h"
#include "master_process.h"
#include "message_center.h"
#include "general_def.h"
#include "dji_motor.h"
// bsp
#include "bmi088.h"
#include "TCS34725.h"
#include "servo_motor.h"
#include "bsp_dwt.h"
#include "bsp_log.h"
#include "arm_math.h"

#define L_CENTER (100.0f * DEGREE_2_RAD)
#define constant_speed 3000.0f // 常速

/* cmd应用包含的模块实例指针和交互信息存储*/
static RC_ctrl_t *rc_data;              // 遥控器数据,初始化时返回
static Vision_Recv_s *vision_recv_data; // 视觉接收数据指针,初始化时返回
static Vision_Send_s vision_send_data;  // 视觉发送数据
static attitude_t *IMU_data;    // IMU数据

/* 运动机构信息存储*/
static ServoInstance *servo0_motor, *servo1_motor, *servo2_motor, *servo3_motor;;
static DJIMotorInstance *motor_lift, *motor_right;

static PIDInstance angle_pid;   // 角度PID实例
float angle_pid_output;         // 角度PID输出
float speed_forward;            // 前进速度

/* 机器人状态信息存储*/
static Robot_Status_e robot_state; // 机器人整体工作状态
static Robot_Camp_e camp;

/* 共享数据存储 */
float time_now; // 当前时间

/* 测试数据存储*/
int16_t servo_angle_test = 60;

/* 临时数据存储*/
Holder_s servo0_holder, servo1_holder, servo2_holder, coord_holder, discard_holder; // coord,滤除坐标误归零，discard,等待丢弃识别稳定

static int16_t vision_count;
static uint8_t frame_use;
static float frame_offset;
static float servo0_end, servo1_end, servo2_end, coord_end, discard_end; // 各个机构结束标志
static float recv_x, recv_y;
static float vy_last, wz_last;  // 过往速度
static float vt_lift, vt_right; // 解算轮速临时数据

/* 比赛信息记录*/
typedef enum
{
    match_wait_start = 0,  // 等待开局
    match_aimless_forward, // 无目标直行
    match_first_load_frame,      // 首次全下框架
    match_first_discard_ball,    // 首次丢弃球
    match_transition,            // 过渡阶段
} Match_Manager_e;

static Match_Manager_e match_manager[2] = {match_wait_start, match_wait_start};

float Ramp_calc(float input, float last_output, float increment)
{
    if(input > last_output + increment)
        last_output = last_output + increment;
    else if(input < last_output - increment)
        last_output = last_output - increment;
    else
        last_output = input;
    return last_output;
}

void Holder_init(Holder_s *holder, float shadow)
{
    holder->time_hold_shadow = shadow;
}

int8_t Holder_check(Holder_s *holder)
{   
    if (holder->time_hold_shadow > 0.0f)
    {
        if (time_now - holder->time_start > holder->time_hold)
        {
            holder->time_hold = 0.0f;
            return 1;   //解除挂起
        }
        else return 0;  //等待释放
    }
    else return 1;  //不挂起
}

void Wait_control()
{
    servo0_end = Holder_check(&servo0_holder);
    servo1_end = Holder_check(&servo1_holder);
    servo2_end = Holder_check(&servo2_holder);
    coord_end = Holder_check(&coord_holder);
    discard_end = Holder_check(&discard_holder);

    if(coord_end == 1)
    {
        recv_x = 0.0f;
        recv_y = 0.0f;
    }
}

void Servo_init()
{
    Servo_Motor_FreeAngle_Set(servo0_motor, Servo0_Up_Angle);
    servo0_holder.time_hold = servo0_holder.time_hold_shadow;

    Servo_Motor_FreeAngle_Set(servo1_motor, Servo1_Up_Angle);
    servo1_holder.time_hold = servo1_holder.time_hold_shadow;

    Servo_Motor_FreeAngle_Set(servo2_motor, Servo2_Up_Angle);
    servo2_holder.time_hold = servo2_holder.time_hold_shadow;

    Servo_Motor_FreeAngle_Set(servo3_motor, Servo3_Up_Angle);
}

void Vision_message_manager()
{
    if((vision_recv_data->x != 0.0f) || (vision_recv_data->y != 0.0f))
    {
        recv_x = vision_recv_data->x;
        recv_y = vision_recv_data->y;
        coord_holder.time_hold = coord_holder.time_hold_shadow;
    }
    if(vision_recv_data->frame_state[1] == 0)
            frame_use = 1;
    else if(vision_recv_data->x > 0.0f)
    {
        if(vision_recv_data->frame_state[0] == 0)
            frame_use = 0;
        else frame_use = 2;
    }
    else if(vision_recv_data->x < 0.0f)
    {
        if(vision_recv_data->frame_state[2] == 0)
            frame_use = 2;
        else frame_use = 0;
    }
}

void Automode_manager()
{
    if(match_manager[TEMP] == match_wait_start)
    {
        // 等待开局
        if (HAL_GPIO_ReadPin(GPIOI, GPIO_PIN_6) == GPIO_PIN_SET)
        {
            match_manager[TEMP] = match_aimless_forward;
            DJIMotorSetRef(motor_lift, 10000.0f);
            DJIMotorSetRef(motor_right, 10000.0f);
        }
    }
    else if(match_manager[TEMP] == match_aimless_forward)
    {
        // 无目标直行
        if((motor_lift->measure.total_angle + motor_right->measure.total_angle) > 20000.0f)
        {
            Servo_Motor_FreeAngle_Set(servo0_motor, Servo0_Down_Angle);
            servo0_holder.time_hold = servo0_holder.time_hold_shadow;
            Servo_Motor_FreeAngle_Set(servo1_motor, Servo1_Down_Angle);
            servo1_holder.time_hold = servo1_holder.time_hold_shadow;
            Servo_Motor_FreeAngle_Set(servo2_motor, Servo2_Down_Angle);
            servo2_holder.time_hold = servo2_holder.time_hold_shadow;
            match_manager[TEMP] = match_first_load_frame;
            servo0_holder.time_hold = servo0_holder.time_hold_shadow;
        }
    }
    else if(match_manager[TEMP] == match_first_load_frame)
    {
        // 首次下框架
        if(servo0_end == 1)
        {
            match_manager[TEMP] = match_first_discard_ball;
            discard_holder.time_hold = discard_holder.time_hold_shadow;
        }
    }
    else if(match_manager[TEMP] == match_first_discard_ball)
    {
        // 丢弃球
        if(discard_end == 1)
        {
            if(vision_recv_data->frame_state[0] == 2)
                Servo_Motor_FreeAngle_Set(servo0_motor, Servo0_Up_Angle);
            if (vision_recv_data->frame_state[1] == 2)
                Servo_Motor_FreeAngle_Set(servo1_motor, Servo1_Up_Angle);
            if (vision_recv_data->frame_state[2] == 2)
                Servo_Motor_FreeAngle_Set(servo2_motor, Servo2_Up_Angle);
            
            match_manager[TEMP] = match_transition;
        }
    }
    else if(match_manager[TEMP] == match_transition)
    {
        if ((vision_recv_data->state == 0) && (vision_recv_data->is_have == 1))
        {
            // 目标为球,to ball
            if(vision_recv_data->frame_state[0] == 1)
                Servo_Motor_FreeAngle_Set(servo0_motor, Servo0_Down_Angle);
            else
                Servo_Motor_FreeAngle_Set(servo0_motor, Servo0_Up_Angle);
            if(vision_recv_data->frame_state[1] == 1)
                Servo_Motor_FreeAngle_Set(servo1_motor, Servo1_Down_Angle);
            else
                Servo_Motor_FreeAngle_Set(servo1_motor, Servo1_Up_Angle);
            if(vision_recv_data->frame_state[2] == 1)
                Servo_Motor_FreeAngle_Set(servo2_motor, Servo2_Down_Angle);
            else
                Servo_Motor_FreeAngle_Set(servo2_motor, Servo2_Up_Angle);

            // 接近目标
            if(frame_use == 0)
                frame_offset = 100.0f;
            else if(frame_use == 1)
                frame_offset = 0.0f;
            else if(frame_use == 2)
                frame_offset = -100.0f;

            angle_pid_output = PIDCalculate(&angle_pid, recv_x + frame_offset, 0.0f);
            if(recv_y < 100.0f)
                speed_forward = constant_speed *  (recv_y / 100.0f);
            else
                speed_forward = constant_speed;

            DJIMotorSetRef(motor_lift, speed_forward - angle_pid_output);
            DJIMotorSetRef(motor_right, speed_forward + angle_pid_output);
        }
        else if ((vision_recv_data->state == 1) && (vision_recv_data->is_have == 1))
        {
            // 目标为安全区,to safe
            angle_pid_output = PIDCalculate(&angle_pid, recv_x, 0.0f);
            if (recv_y < 100.0f)
                speed_forward = constant_speed * (recv_y / 100.0f);
            else
                speed_forward = constant_speed;

            DJIMotorSetRef(motor_lift, speed_forward - angle_pid_output);
            DJIMotorSetRef(motor_right, speed_forward + angle_pid_output);
        }
        else if (vision_recv_data->is_have == 0)
        {
            // 丢失,lost
            //开始寻找目标
            DJIMotorSetRef(motor_lift, 2000.0f);
            DJIMotorSetRef(motor_right, -2000.0f);
            // servo3_angle = 70 + (int32_t)(30.0f *arm_sin_f32((((int32_t)(time_now) % 1000) / 1000.0f) * (2.0f * PI) ));
            // Servo_Motor_FreeAngle_Set(servo3_motor,servo3_angle);
        }
    }
}

void RobotCMDInit()
{
    //初始化信息实例
    rc_data = RemoteControlInit(&huart3);   // 修改为对应串口,注意如果是自研板dbus协议串口需选用添加了反相器的那个
    vision_recv_data = VisionInit(&huart6); // 视觉通信串口
    IMU_data = INS_Init();

    //初始化运动机构实例
    Servo_Init_Config_s config = {
        .htim = &htim1,
        .Servo_Angle_Type = Free_Angle_mode,
        .Servo_type = Servo180,
    };
    config.Channel = TIM_CHANNEL_1;
    servo0_motor = ServoInit(&config);
    config.Channel = TIM_CHANNEL_2;
    servo1_motor = ServoInit(&config);
    config.Channel = TIM_CHANNEL_3;
    servo2_motor = ServoInit(&config);
    config.Channel = TIM_CHANNEL_4;
    servo3_motor = ServoInit(&config);

    Motor_Init_Config_s chassis_motor_config = {
        .can_init_config.can_handle = &hcan1,
        .controller_param_init_config = {
            .speed_PID = {
                .Kp = 0.4,
                .Ki = 0,
                .Kd = 0,
                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                .MaxOut = 10000,
            },
        },
        .controller_setting_init_config = {
            .angle_feedback_source = MOTOR_FEED,
            .speed_feedback_source = MOTOR_FEED,
            .outer_loop_type = SPEED_LOOP,
            .close_loop_type = SPEED_LOOP,
        },
        .motor_type = M2006,
    };
    //  @todo: 当前还没有设置电机的正反转,仍然需要手动添加reference的正负号,需要电机module的支持,待修改.
    chassis_motor_config.can_init_config.tx_id = 1;
    chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
    motor_lift = DJIMotorInit(&chassis_motor_config);

    chassis_motor_config.can_init_config.tx_id = 2;
    chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
    motor_right = DJIMotorInit(&chassis_motor_config);

    PID_Init_Config_s angle_pid_config = {
        .Kp = 10,
        .Ki = 0,
        .Kd = 0,
        .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
        .MaxOut = 20000,
    };
    PIDInit(&angle_pid, &angle_pid_config);

    // 初始化工具
    Holder_init(&servo0_holder, 300.0f);
    Holder_init(&servo1_holder, 300.0f);
    Holder_init(&servo2_holder, 300.0f);
    Holder_init(&coord_holder, 100.0f);

    //初始化运动机构状态
    Servo_init();

    robot_state = ROBOT_READY; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入

    vision_send_data.head = 0xF0;
    vision_send_data.last = 0x0F;
}

/**
 * @brief 控制输入为遥控器(调试时)的模式和控制量设置
 *
 */
static void RemoteControlSet()
{

    vy_last = Ramp_calc((float)(rc_data[TEMP].rc.rocker_r1), vy_last, 20.0f);
    wz_last = Ramp_calc((float)(rc_data[TEMP].rc.rocker_r_), wz_last, 20.0f);

    vt_lift = vy_last - wz_last * 0.8f;
    vt_right = vy_last + wz_last * 0.8f;
    
    DJIMotorSetRef(motor_lift, vt_lift * 150.0f);
    DJIMotorSetRef(motor_right, vt_right * 150.0f);

    if (switch_is_up(rc_data[TEMP].rc.switch_right))
    {
        if(rc_data[TEMP].rc.rocker_l_ > 500)
        {
            Servo_Motor_FreeAngle_Set(servo0_motor, Servo0_Down_Angle);
            servo0_holder.time_hold = servo0_holder.time_hold_shadow;
        }
        else if (rc_data[TEMP].rc.rocker_l_ < -500)
        {
            Servo_Motor_FreeAngle_Set(servo0_motor, Servo0_Up_Angle);
            servo0_holder.time_hold = servo0_holder.time_hold_shadow;
        }
    }
    else if (switch_is_mid(rc_data[TEMP].rc.switch_right))
    {
        if (rc_data[TEMP].rc.rocker_l_ > 500)
        {
            Servo_Motor_FreeAngle_Set(servo1_motor, Servo1_Down_Angle);
            servo1_holder.time_hold = servo1_holder.time_hold_shadow;
        }
        else if (rc_data[TEMP].rc.rocker_l_ < -500)
        {
            Servo_Motor_FreeAngle_Set(servo1_motor, Servo1_Up_Angle);
            servo1_holder.time_hold = servo1_holder.time_hold_shadow;
        }
    }
    else if (switch_is_down(rc_data[TEMP].rc.switch_right))
    {
        if (rc_data[TEMP].rc.rocker_l_ > 500)
        {
            Servo_Motor_FreeAngle_Set(servo2_motor, Servo2_Down_Angle);
            servo2_holder.time_hold = servo2_holder.time_hold_shadow;
        }
        else if (rc_data[TEMP].rc.rocker_l_ < -500)
        {
            Servo_Motor_FreeAngle_Set(servo2_motor, Servo2_Up_Angle);
            servo2_holder.time_hold = servo2_holder.time_hold_shadow;
        }
    }
    if (rc_data[TEMP].rc.rocker_l1 < -400)
    {
        Servo_Motor_FreeAngle_Set(servo0_motor, Servo0_Down_Angle);
        servo0_holder.time_hold = servo0_holder.time_hold_shadow;
        Servo_Motor_FreeAngle_Set(servo1_motor, Servo1_Down_Angle);
        servo1_holder.time_hold = servo1_holder.time_hold_shadow;
        Servo_Motor_FreeAngle_Set(servo2_motor, Servo2_Down_Angle);
        servo2_holder.time_hold = servo2_holder.time_hold_shadow;
    }
    else if (rc_data[TEMP].rc.rocker_l1 > 400)
    {
        Servo_Motor_FreeAngle_Set(servo0_motor, Servo0_Up_Angle);
        servo0_holder.time_hold = servo0_holder.time_hold_shadow;
        Servo_Motor_FreeAngle_Set(servo1_motor, Servo1_Up_Angle);
        servo1_holder.time_hold = servo1_holder.time_hold_shadow;
        Servo_Motor_FreeAngle_Set(servo2_motor, Servo2_Up_Angle);
        servo2_holder.time_hold = servo2_holder.time_hold_shadow;
    }
}

static void StartHandler()
{
    // if (HAL_GPIO_ReadPin(START_KEY_GPIO_Port,START_KEY_Pin) == GPIO_PIN_SET)
    // {
    //     //添加启动部分
    //     DJIMotorEnable(motor_lift);
    //     DJIMotorEnable(motor_right);
    //     robot_state = ROBOT_READY;

    //     LOGINFO("[CMD] reinstate, robot ready");
    // }
    if (switch_is_mid(rc_data[TEMP].rc.switch_left))
    {
        //添加启动部分
        DJIMotorEnable(motor_lift);
        DJIMotorEnable(motor_right);
        robot_state = ROBOT_READY;

        LOGINFO("[CMD] reinstate, robot ready");
    }
    else if (!(switch_is_mid(rc_data[TEMP].rc.switch_left)))
    {
        // 添加启动部分
        DJIMotorStop(motor_lift);
        DJIMotorStop(motor_right);
        robot_state = ROBOT_STOP;

        LOGINFO("[CMD] reinstate, robot stop");
    }
}

/* 机器人核心控制任务,200Hz频率运行(必须高于视觉发送频率) */
void RobotCMDTask()
{
    time_now = DWT_GetTimeline_ms(); // 获取当前时间

    StartHandler();

    if (HAL_GPIO_ReadPin(GPIOC,GPIO_PIN_6) == GPIO_PIN_RESET)
    {
        camp = RED_CAMP;
        vision_send_data.team = 0;
    }
    else
    {
        camp = BLUE_CAMP;
        vision_send_data.team = 1;
    }

    if (robot_state)
    {
        // 处理主逻辑
        Vision_message_manager();
        Wait_control();
        Automode_manager();
        ServeoMotorControl();
        if (switch_is_mid(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[中],遥控器控制
        {
            RemoteControlSet();
        }
    }

    // Servo_Motor_FreeAngle_Set(servo0_motor, servo_angle_test);
    // ServeoMotorControl();
    match_manager[LAST] = match_manager[TEMP]; // 保存当前状态
    // 填充视觉信息
    vision_count++;
    if (vision_count >= 40)
    {
        vision_count = 0;
        VisionSend(&vision_send_data);
    }
}
